/**
 * Author: luoqi
 * Created Date: 2025-10-14 00:06:14
 * Last Modified: 2025-11-26 00:36:9
 * Modified By: luoqi at <**@****>
 * Copyright (c) 2025 <*****>
 * Description:
 */

#ifndef _POWERFLOW_H_
#define _POWERFLOW_H_

#include <functional>
#include <vector>
#include <thread>
#include <mutex>

class PowerFlow {
public:
    enum MotorType {
        R56 = 0,
        R86_2,
        R86_3,
    };

    enum ErrCode {
        NONE = 0,
        OVER_VOLT = 2,
        UNDER_VOLT = 4,
        OVER_VEL = 8,
        OVER_THM = 16,
        OVER_CUR = 32,
        CALIB_ERR = 256,
        UNCALIB = 512,
    };

    enum State {
        DISABLE = 0x00,
        ENABLE = 0x01,
    };

    enum Mode {
        ILOOP = 0,
        ILOOP_TRAPEZOID = 1,
        VLOOP = 2,
        VLOOP_TRAPEZOID = 3,
        PLOOP = 4,
        PLOOP_TRAPEZOID = 5,
        MIT = 6,
    };

    struct Motor {
        uint16_t id{ 0 };
        MotorType type{ R56 };
        Mode mode{ MIT };
        float pos{ 0.0f };
        float vel{ 0.0f };
        float cur{ 0.0f };

        ErrCode err{ NONE };
        State state{ DISABLE };
        bool hb{ false };
    };

    using SendPort = std::function<int(uint16_t id, void *data, uint8_t sz)>;
    using RecvPort = std::function<int(uint16_t &id, void *buffer, uint8_t &sz, int timeout_ms)>;
    using CallBack = std::function<void(void *param)>;
    using MitMsg = uint8_t[8];

public:
    PowerFlow();
    ~PowerFlow();

    int port_set(SendPort send, RecvPort recv);

    int enable(int joint);

    int disable(int joint);

    int mode_set(int joint, Mode mode);

    int cur_set(int joint, float cur);

    int vel_set(int joint, float vel);

    int pos_set(int joint, float pos);

    int mit_set(int joint, float pos, float vel, float tor, float kp, float kd);

    // int id_set(int joint, uint16_t id);

    // int config_save(int joint);

    // int zero_set(int joint);

    int recv_decode(int timeout_ms);

    int decode(uint16_t id, void *data, size_t sz);

    int poll();

public:
    int num() const { return joints_.size(); }

    const std::vector<Motor> &info() const { return joints_; }

private:
    SendPort send_{ nullptr };
    RecvPort recv_{ nullptr };
    CallBack err_cb_{ nullptr };

    MitMsg mit_msgs_[8]{ 0 };
    std::mutex mtx_;

    std::vector<Motor> joints_;

    int fp2uint_(float x, float min, float max, int bits);
    float uint2fp_(int x, float min, float max, int bits);
    void mit_pack_(MitMsg &frame, float pos, float vel, float tor, float kp, float kd);
};

#endif
